/*
 * main.cpp
 *
 *  Created on: 2011. 1. 4.
 *      Author: robotis
 */

#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <libgen.h>
#include <cmath>
#include "include/Matrix.h"
#include "include/MotorController.h"
#include "include/ATMega328p.h"


#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/nonfree/nonfree.hpp"
#include "opencv2/imgproc/imgproc.hpp"


//#include "Camera.h"
//#include "mjpg_streamer.h"
#include "include/LinuxDARwIn.h"

#define INI_FILE_PATH       "/home/mul8/Documents/config.ini"
//#define U2D_DEV_NAME        "/dev/ttyUSB0"

using namespace Robot;
using namespace std;
using namespace cv;


int main(void)
{
    printf( "\n===== Ball following Tutorial for DARwIn =====\n\n");


    minIni* ini = new minIni(INI_FILE_PATH);
//TODO debug statement
    BallFollower follower = BallFollower();
	follower.DEBUG_PRINT = false;

	if(MotionManager::GetInstance()->Initialize(/*&cm730*/) == false)
	{
		printf("Fail to initialize Motion Manager!\n");
			return 0;
	}

//TODO User Input
	printf("Press ENTER to move into Initial Pose!\n");
	getchar();

    MotionManager::GetInstance()->LoadINISettings(ini);
    //printf("loaded ini for MotionManager\n");
    Walking::GetInstance()->LoadINISettings(ini);
    //printf("loaded ini for Walking\n");

	//MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance());
	MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance());
//    LinuxMotionTimer *motion_timer = new LinuxMotionTimer(MotionManager::GetInstance());
//    motion_timer->Start();

	/////////////////////////////////////////////////////////////////////

	int n = 0;
	//int param[JointData::NUMBER_OF_JOINTS * 5];
	std::vector<int> wStartPosition, wGoalPosition,wGoalSpeed, wMaxTorque, ids;

	wStartPosition.resize(JointData::NUMBER_OF_JOINTS);
	wMaxTorque.resize(JointData::NUMBER_OF_JOINTS);
	wGoalPosition.resize(JointData::NUMBER_OF_JOINTS);
	wGoalSpeed.resize(JointData::NUMBER_OF_JOINTS);
	ids.resize(JointData::NUMBER_OF_JOINTS);

	//int wGoalPosition, wStartPosition, wDistance;

	for(int id=JointData::ID_R_SHOULDER_PITCH; id<JointData::NUMBER_OF_JOINTS; id++)
	{
		ids[n]= id;
		wStartPosition[n]= MotorController::GetInstance()->readMotorPosition(id);
		if(id==5 || id==6){//dont let elbow motors have high torque
			wMaxTorque[n]= 0;
		}
		else{
		wMaxTorque[n]= 1023;
		}

		if(id== 15){//right ankle pitch was leaning body forward
			wGoalPosition[n]= 2048-100;

		}
		else if(id==16){//left ankle pitch was leaning body forward
			wGoalPosition[n]= 2048+100;

		}
		else{
		wGoalPosition[n]= 2048;
		}
		wGoalSpeed[n]= .1285*abs(wStartPosition[n]-wGoalPosition[n])/2;
		std::cout<<"Setting "<<ids[n]<<" to "<<wGoalPosition[n]<<" with speed "<< wGoalSpeed[n]<<" torque = "<<wMaxTorque[n]<<std::endl;
		n++;
	}
	MotorController::GetInstance()->sendSyncWrite(ids, MX28::P_MOVING_SPEED_L, MX28::P_WORD, wGoalSpeed);
	MotorController::GetInstance()->sendSyncWrite(ids, MX28::P_GOAL_POSITION_L, MX28::P_WORD, wGoalPosition);
	MotorController::GetInstance()->sendSyncWrite(ids, MX28::P_TORQUE_LIMIT_L, MX28::P_WORD, wMaxTorque);

	//TODO
	//MotorController::GetInstance()->SyncWrite(MX28::P_GOAL_POSITION_L, 5, JointData::NUMBER_OF_JOINTS - 1, param);

//	printf("Press the ENTER key to begin!\n");
//	getchar();
	
  //  Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
    Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);
	//printf("Enabled walking to make calculations on body\n");

//TODO
	MotionManager::GetInstance()->SetEnable(true);
//printf("Enabled the MotionManager to take effect on motors\n");
	int left_right_holder = 45;  //between -30 and 30 degrees
	int left_right = 0;  //between -30 and 30 degrees
	int front_back_holder= 10;
	int front_back= -45;  //greater than -68, up to 0 degrees

	int process_counter= 0;
//	std::string input;

//TODO for controlling walking with user input
	printf("\nPress enter to start walking:\n");
	printf("\nTo exit at any time, click on Walk Tuning window and type 'x'\n");



	cvNamedWindow("Walk Direction and Speed", cv::WINDOW_NORMAL);

	cvCreateTrackbar("Forward", "Walk Direction and Speed", &front_back_holder, 67, NULL);
	cvCreateTrackbar("Right -- Left", "Walk Direction and Speed", &left_right_holder, 90, NULL);

	waitKey(1);
	//char input = getchar();

	static struct timespec next_time;
	clock_gettime(CLOCK_MONOTONIC,&next_time);

//TODO	ATMega328p::GetInstance()->initialize();
	getchar();

	while ((char)waitKey(1) != 'x')
	{


		next_time.tv_sec += (next_time.tv_nsec + MotionModule::TIME_UNIT * 1000000) / 1000000000;
		next_time.tv_nsec = (next_time.tv_nsec + MotionModule::TIME_UNIT * 1000000) % 1000000000;

		left_right= left_right_holder - 45;
		front_back= front_back_holder*-1;

		Point2D pos; // x and y coordinates (only used for when the ball is super close)

		//	TODO try to stop the walk before it goes backwards
//		if(process_counter == 4200){
//			MotionManager::GetInstance()->RemoveModule((MotionModule*)Walking::GetInstance());
//			printf("Removing walking module\n");
//			MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance());
//			process_counter=0;
//
//		}

		follower.Process(pos, left_right, front_back); //determine how the robot should walk or not walk

		MotionManager::GetInstance()->Process(); //run motion manager to make robot walk/move
		clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL);//focus is on next_time (built a few lines above) So that it does not run continuously

//		if ((char)waitKey(3) == 'f'){
//			std::cout<<"process_counter= "<<process_counter<<std::endl;
//		}


		//This function call uses the ball position to determine the kicking options, not the walking
		//The Walking (following) is decided based on the tilt/pan percentage of the head
		//pan range is -80(left) to 80(right) degrees
		//tilt range is -68(down) to 0(straight ahead) degrees
		process_counter++;

	}
	printf("\nWould you like to save your parameter changes? (Y/N)\t");
	std::string decision;
	std::cin >> ws;
	std::getline(std::cin, decision);
	if(decision=="Y" || decision=="y"){
		Walking::GetInstance()->SaveINISettings(ini);
		printf("\n\nSaving parameters! \n");
	}

	else{
		printf("\n\nExiting without saving \n");

	}

    return 0;
}
